/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.iaroc.irobot.sample;

import com.iaroc.irobot.*;
import com.iaroc.irobot.util.XLights;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.*;

/**
 *
 * @author Mark
 */
public class FinalAttemptEventHandler extends IRobotCreateEventHandler
    {
    IRobotCreate create;
    EDemoBoard board;
    MaxSonarEZ1 sonar;
    XLights lights;
    int createVelocity;
    int totalDistance;
    int spikeCounter;
    final int HALF_NOTE = 64;
    final int QUARTER_NOTE = 32;
    final int EIGHT_NOTE = 16;
    final int SIX_NOTE = 8;
    
    private int count;
    private int distanceCount;
    private long lastTime;
    
    private boolean bumpLeft;
    private boolean bumpRight;
    
    public static final int IDEAL_CLOSENESS = 150;//usually 160
    public static final int IDEAL_FARNESS = 1;// usually 1
    public static final int TURN_RADIUS = 110;//Normally 150ish
    public static final int LEFT_LED = 0;
    public static final int MID_LEFT_LED = 1;
    public static final int MID_RIGHT_LED = 6;
    public static final int RIGHT_LED = 7;
    public final double MM_TO_FEET = 0.0032808399;
    public final double FEET_TO_MM = 1/MM_TO_FEET;
    public static final double timePerUnit = 1.963;
    
    public FinalAttemptEventHandler()
        {
        super();
        board = EDemoBoard.getInstance();
        lights = new XLights(board, 2, 5);
        lights.startPsilon();
        sonar = new MaxSonarEZ1(EDemoBoard.A0);
        board.getLEDs()[LEFT_LED].setColor(LEDColor.BLUE);
        board.getLEDs()[MID_LEFT_LED].setColor(LEDColor.RED);
        board.getLEDs()[MID_RIGHT_LED].setColor(LEDColor.GREEN);
        create = new IRobotCreate();
        totalDistance = 0;
        lights.setColor(LEDColor.WHITE);
        create.setVelocity(500);
        createVelocity = 500;
        distanceCount = 0;
        spikeCounter = 0;
        System.out.println("Create velocity is: " + create.getVelocity());
        lastTime = System.currentTimeMillis();
        }
    
    public void pollSensors()
        {
        wallSonarEvent(sonar.getDistance());
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, this);
        }

    public void wallSignalEvent(int oldInt, int wallSignal) 
        {
        count++;
        boolean mid = false, left = false, right = false;
        super.wallSignalEvent(oldInt, wallSignal);
        //System.out.println("Wall signal is: " + wallSignal);
        if(wallSignal >= IDEAL_FARNESS && wallSignal <= IDEAL_CLOSENESS)
            {
            mid = true;
            create.goForward();
            }
        if (wallSignal > IDEAL_FARNESS && !mid)
            {
            left = true;
            create.drive(createVelocity, TURN_RADIUS);
            }
        if (wallSignal < IDEAL_CLOSENESS && !mid && !left)
            {
            right = true;
            create.drive(createVelocity, -TURN_RADIUS);
            }
        board.getLEDs()[LEFT_LED].setOn(mid);
        board.getLEDs()[MID_LEFT_LED].setOn(left);
        board.getLEDs()[MID_RIGHT_LED].setOn(right);
        }
    
    public void distanceEvent(int oldInt, int distance) 
        {
        super.distanceEvent(oldInt, distance);
        distanceCount += distance;
        }    
    
    public void bumpLeftEvent(boolean oldBoolean, boolean bumpLeft) 
        {
        super.bumpLeftEvent(oldBoolean, bumpLeft);
        this.bumpLeft = bumpLeft;        
        }

    public void bumpRightEvent(boolean oldBoolean, boolean bumpRight) 
        {
        super.bumpRightEvent(oldBoolean, bumpRight);
        this.bumpRight = bumpRight;
        }
    
    public void wallSonarEvent(int sonarSignal)
        {
        System.out.println("Sonar Signal: " + sonarSignal);
        spikeCounter = sonarSignal > 100? spikeCounter + 1: 0;
        if (spikeCounter > 1)
            {
            System.out.println("Distance adding 2");
            spikeCounter = 0;
            totalDistance *= MM_TO_FEET;
            totalDistance += 2;
            totalDistance *= FEET_TO_MM;
            }   
        }
    
    public long timeEvent(long lastTime, long newTime)
        {
        long delta = newTime - lastTime;
        this.lastTime = newTime;
        return delta;
        }
    
    public void eventBatchStart() 
        {
        super.eventBatchStart();        
        count = 0;
        }
    
    public void eventBatchEnd() 
        {
        super.eventBatchEnd();
        boolean straight = count <= 0;
        totalDistance += create.getDistance();
        if (bumpLeft)
            {
            totalDistance *= MM_TO_FEET;//to make it into feet
            System.out.println("Distance travled was: " + totalDistance);
            totalDistance = 0;
            create.spinLeft();
            create.waitAngle(60);
            create.stop();
//            long time = timeEvent(lastTime, System.currentTimeMillis());
//            System.out.println("Time it took was: " + time);
//            System.out.println("The units it took was: " + (time/1000)/timePerUnit);
            }
        
        if (bumpRight && !bumpLeft)
            {
            create.spinLeft();
            create.waitAngle(4);
            create.stop();
            }
        
        if(straight && !bumpLeft && !bumpRight)// normally just if(straight) testing to see if
            //this is better so we dont ram wall as much sometimes
            create.drive(createVelocity, -TURN_RADIUS - 10);
        }
    }
